//最早的代码
#include "ThreadObject.h"
#include <QThread>
#include <QDebug>
#include <QMutexLocker>
#include <QElapsedTimer>

#include <QTimer>
#include <QEventLoop>
#include <limits>

#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sched.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <time.h>
#include <sys/mman.h>
#include <malloc.h>
#include <stdlib.h>
#include <pthread.h>
#include <stdarg.h>
#include <sys/ipc.h>
#include <sys/msg.h>
#include <pthread.h>

//USING QT
#include <iostream>
//#include "mainwindow_consoler.h"
#include <thread>
#include <iostream>
#include <mutex>
//#include <QApplication>
//#include <QDesktopWidget>
#include "./hardware_interface/Motor_API/function.h"

#include "ecrt.h"
#define     Bool                              int
#define     false                             0
#define     true                              1
#define     FALSE                             0
#define     TRUE                              1

#define     MAX(a, b) (a>b)?(a):(b)
#define     ETHERCAT_STATUS_OP                0x08
#define     STATUS_SERVO_ENABLE_BIT           (0x04)

#define FREQUENCY 500
#define     NSEC_PER_SEC         (1000000000L)

#define  CLOCK_TO_USE  CLOCK_REALTIME

//4MS
int PERIOD_NS = 4000000;//4000000
int PERIOD_NS_HALF = PERIOD_NS/2;

#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + (B).tv_nsec - (A).tv_nsec)
#define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)

//0-99 只测试数字量模拟量
//1=ZhiAngTech_EtherCAT_16X16Y
//2=ZhiAngTech_EtherCAT_16X16Y+ZhiAngTech_EtherCAT_8AD4DAV1.2
//.............................................................................................................
//11=BECKHOFF_EL1809
//12=BECKHOFF_EL1809+EL2809
//13=BECKHOFF_EL1809+EL2809+EL3104
//14=BECKHOFF_EL1809+EL2809+EL3104+EL4034


//100-199 测试单独伺服
//100=HUICHUAN-axis 1
//101=HUICHUAN-axis 2
//103=HUICHUAN-axis2+OMULONG-axis1

//110=MC5004-axis 1


//200-299 测试多轴机器人
//200=DOBOT-axis 6
//201=DOBOT-axis 6 + io-3(2 in box and 1 in tool) will result to board shutdown joint power supply
//202=ZHIANG 8DI 8DO+DOBOT-axis 6
//.............................................................................................................
//210=SERVOTRINIX-axis 4(ST60)
//211=SERVOTRINIX-axis 6(SV45)
//.............................................................................................................
//220=LEISAI-axis 6(TEST)
//.............................................................................................................
//230=ZEROERR-axis 6(TEST)


//300-399 测试其他总线配置情况


#define TEST_ROBOT_TYPE     114



//测试的IO板类型，不考虑混用，只用一个品牌的（即便你不用也不能不定义）
//不同IO的区别参考"ec_slave_io.c"
//1=越疆控制柜自带的IO
//2=佛山智昂的IO
//3=倍福IO
#define TEST_IOBOARD_TYPE   2


#define DO_MEASURE   0

int test_di_count=0;
int set_do_count=0;
#define DO_TEST_DIDO_AUTO   0
#define DO_TEST_AIAO_AUTO   0



#define DO_MEASURE   0

//master status
typedef enum  _SysWorkingStatus
{
    SYS_WORKING_POWER_ON,//=0
    SYS_WORKING_SAFE_MODE,//=1
    SYS_WORKING_OP_MODE,//=2
    SYS_WORKING_LINK_DOWN,//=3   //暂时没用到
    SYS_WORKING_IDLE_STATUS//=4       //系统空闲(不再需要)
}SysWorkingStatus;

typedef  struct  _GSysRunningParm
{
    SysWorkingStatus   m_gWorkStatus;
}GSysRunningParm;

GSysRunningParm    gSysRunning;

int run = 1;
int ecstate = 0;

//input is linux time struct including seconds and nanoseconds
#define     TIMESPEC2NS(T)       ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)

uint64_t dc_start_time_ns,dc_time_ns;
struct timespec next_period;
static int64_t  system_time_base = 0LL;
static uint64_t wakeup_time = 0LL;
static uint64_t overruns = 0LL;
ec_master_t *master = NULL;
static ec_master_state_t master_state = {};


// EtherCAT
static ec_domain_t *domainServoInput = NULL;
static ec_domain_state_t domainServoInput_state = {};
static ec_domain_t *domainServoOutput = NULL;
static ec_domain_state_t domainServoOutput_state = {};

static uint8_t *domainOutput_pd = NULL;
static uint8_t *domainInput_pd = NULL;

static ec_slave_config_t *sc_Dobot[10];
static ec_slave_config_state_t sc_Dobot_state[10];

//不管是数字量还是数字量+模拟量，都认为是IO板（部分IO是数字+模拟）
static ec_slave_config_t *sc_Dobot_IO[10];
static ec_slave_config_state_t sc_Dobot_IO_state[10];


//所有输入
static unsigned int  statusword[10];
static unsigned int  errorcode[10];
static unsigned int  actpos[10];
static unsigned int  actvel[10];
static unsigned int  acttorque[10];
static unsigned int  actposfollowerror[10];
//static unsigned int  modeofoperationdisplayed[10];//8bit特别注意

//所有输出
static unsigned int  controlword[10];
static unsigned int  homingmethod[10];
static unsigned int  targetposition[10];
static unsigned int  targetvelocity[10];
static unsigned int  targettorque[10];
static unsigned int  modeofoperation[10];//8bit特别注意

//dido data
static unsigned int  DigitalBoardOUT[10];
static unsigned int  DigitalBoardIN[10];

//从站IO板
//#include "ec_slave_io.c"

//伺服配置
//#include "ec_slave_servo.c"
//#include "ec_slave_servo_000.c"
//#include "ec_slave_servo_200.c"
#include "ec_slave_servo_100.c"


static ec_pdo_entry_info_t Dobot_pdo_entries_output[] = {
    { 0x6040, 0x00, 16 }, //Controlword

    //???
    { 0x607a, 0x00, 32 },  //TargetPosition

    //{ 0x60ff, 0x00, 32 },  //TargetVelocity
    //{ 0x6071, 0x00, 16 },  //TargetTorque

    //实际也要6个，但是8个bit的要填充一个(搞错了可能伺服无法上使能，或者能上使能不能转动)
    //{0x6060, 0x00,  8}, // Modes of operation
    //{0x6098, 0x00,  8}, // Homing method,虽然不用，但是需要一个有效的东西8bit填充

};

static ec_pdo_entry_info_t Dobot_pdo_entries_input[] = {
    { 0x6041, 0x00, 16 }, //Statusword
    //{ 0x603f, 0x00, 16 }, //Errorcode

    //???
    { 0x6064, 0x00, 32 }, //CurrentPosition

    //{ 0x606c, 0x00, 32 }, //CurrentVelocity
    { 0x6077, 0x00, 16 }, //CurrentTorque
    //{ 0x60f4, 0x00, 32 }, //CurrentFollowError

};


//RxPDO
static ec_pdo_info_t Dobot_pdo_1600[] = {
    //{ 0x1600, 5, Dobot_pdo_entries_output },//这里参数为2表示塞入2个变量,在倍福里面是0x1600-0x1603分开的，这里合并到一个box

    //{ 0x1600, 1, Dobot_pdo_entries_output },//这里参数为2表示塞入2个变量,在倍福里面是0x1600-0x1603分开的，这里合并到一个box
    { 0x1601, 2, Dobot_pdo_entries_output },//这里参数为2表示塞入2个变量,在倍福里面是0x1600-0x1603分开的，这里合并到一个box


};

//TxPDO
static ec_pdo_info_t Dobot_pdo_1a00[] = {
    //{ 0x1A00, 6, Dobot_pdo_entries_input },//这里参数为3表示塞入3个变量在倍福里面是0x1A00-0x1A03分开的，这里合并到一个box

    //{ 0x1A00, 1, Dobot_pdo_entries_input },//这里参数为3表示塞入3个变量在倍福里面是0x1A00-0x1A03分开的，这里合并到一个box
    //{ 0x1A01, 2, Dobot_pdo_entries_input },//这里参数为3表示塞入3个变量在倍福里面是0x1A00-0x1A03分开的，这里合并到一个box
    { 0x1A01, 3, Dobot_pdo_entries_input },//这里参数为3表示塞入3个变量在倍福里面是0x1A00-0x1A03分开的，这里合并到一个box


};

//all servo is the same
static ec_sync_info_t Dobot_syncs[] = {

    //V1
    //    { 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_ENABLE },
    //    { 1, EC_DIR_INPUT, 0, NULL, EC_WD_ENABLE },
    //{ 2, EC_DIR_OUTPUT, 1, Dobot_pdo_1600, EC_WD_ENABLE },//!!!must enable
    //{ 3, EC_DIR_INPUT, 1, Dobot_pdo_1a00, EC_WD_ENABLE },//!!!must enable

    //V2
    { 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE },
    { 1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE },
    { 2, EC_DIR_OUTPUT, 1, Dobot_pdo_1600, EC_WD_ENABLE },
    { 3, EC_DIR_INPUT, 1, Dobot_pdo_1a00, EC_WD_DISABLE },

    { 0xff }

};


int ConfigPDO()
{
    /********************/
    //printf("xenomai Configuring PDOs...\n");
    //std::cout<<"xenomai Configuring PDOs"<<std::endl;
    domainServoOutput = ecrt_master_create_domain(master);

    if (!domainServoOutput) {
        return -1;
    }
    domainServoInput = ecrt_master_create_domain(master);
    if (!domainServoInput) {
        return -1;
    }
    /********************/
    //printf("xenomai Creating slave configurations...\n");
    //std::cout<<"xenomai Creating slave configurations"<<std::endl;

    //只在特定情况下是引入这个数字模块的
    if(DIDO_FIRST==1)
    {
        //        sc_Dobot_IO[0] =ecrt_master_slave_config(master, Dobot_IOPos0, Dobot_IO0);
        //        if (!sc_Dobot_IO[0]) {
        //            std::cout<<"Failed to get slave configuration IO 0"<<std::endl;
        //            return -1;
        //        }
        //        else
        //        {
        //            std::cout<<"OK to get slave configuration IO 0"<<std::endl;
        //        }

        //        if (ecrt_slave_config_pdos(sc_Dobot_IO[0], EC_END, slave_0_syncs)) {
        //            std::cout<<"Failed to configure PDOs.  IO 0"<<std::endl;
        //            return -1;
        //        }
        //        else
        //        {
        //            std::cout<<"OK to configure PDOs.  IO 0"<<std::endl;
        //        }
    }

    //只在特定情况下是引入这个数字模块的
    if(DIDO_SECODE==1)
    {
        //        sc_Dobot_IO[1] =ecrt_master_slave_config(master, Dobot_IOPos1, Dobot_IO1);
        //        if (!sc_Dobot_IO[1]) {
        //            std::cout<<"Failed to get slave configuration IO 1"<<std::endl;
        //            return -1;
        //        }

        //        if (ecrt_slave_config_pdos(sc_Dobot_IO[1], EC_END, slave_1_syncs)) {
        //            std::cout<<"Failed to configure PDOs.  IO 1"<<std::endl;
        //            return -1;
        //        }
        //        else
        //        {
        //            std::cout<<"OK to get slave configuration IO 1"<<std::endl;
        //        }

    }


    //replace 0=>1 or 2 3....
    //第0套伺服
    if(GLOBAL_AXIS_NUM>=1)
    {
        sc_Dobot[0] =ecrt_master_slave_config(master, Dobot_Pos0, Dobot_Brand0);
        if (!sc_Dobot[0]) {
            //fprintf(stderr, "Failed to get slave configuration Servo J0.\n");
            std::cout<<"Failed to get slave configuration Servo J0"<<std::endl;
            return -1;
        }


        if (ecrt_slave_config_pdos(sc_Dobot[0], EC_END, Dobot_syncs)) {
            //fprintf(stderr, "Failed to configure PDOs. Servo J0.\n");
            std::cout<<"Failed to configure PDOs. Servo J0."<<std::endl;
            return -1;
        }

    }

    if(GLOBAL_AXIS_NUM>=2)
    {
        //第1套伺服
        sc_Dobot[1] =ecrt_master_slave_config(master, Dobot_Pos1, Dobot_Brand1);
        if (!sc_Dobot[1]) {
            //fprintf(stderr, "Failed to get slave configuration Servo J1.\n");
            std::cout<<"Failed to get slave configuration Servo J1"<<std::endl;
            return -1;
        }

        if (ecrt_slave_config_pdos(sc_Dobot[1], EC_END, Dobot_syncs)) {
            //fprintf(stderr, "Failed to configure PDOs. Servo J1.\n");
            std::cout<<"Failed to configure PDOs. Servo J1."<<std::endl;
            return -1;
        }

    }

    if(GLOBAL_AXIS_NUM>=3)
    {
        //第2套伺服
        sc_Dobot[2] =ecrt_master_slave_config(master, Dobot_Pos2, Dobot_Brand2);
        if (!sc_Dobot[2]) {
            //fprintf(stderr, "Failed to get slave configuration Servo J2.\n");
            std::cout<<"Failed to get slave configuration Servo J2"<<std::endl;
            return -1;
        }

        if (ecrt_slave_config_pdos(sc_Dobot[2], EC_END, Dobot_syncs)) {
            //fprintf(stderr, "Failed to configure PDOs. Servo J2.\n");
            std::cout<<"Failed to configure PDOs. Servo J2."<<std::endl;
            return -1;
        }

    }

    if(GLOBAL_AXIS_NUM>=4)
    {
        //第3套伺服
        sc_Dobot[3] =ecrt_master_slave_config(master, Dobot_Pos3, Dobot_Brand3);
        if (!sc_Dobot[3]) {
            //fprintf(stderr, "Failed to get slave configuration Servo J3.\n");
            std::cout<<"Failed to get slave configuration Servo J3"<<std::endl;
            return -1;
        }

        if (ecrt_slave_config_pdos(sc_Dobot[3], EC_END, Dobot_syncs)) {
            //fprintf(stderr, "Failed to configure PDOs. Servo J3.\n");
            std::cout<<"Failed to configure PDOs. Servo J3."<<std::endl;
            return -1;
        }
    }

    if(GLOBAL_AXIS_NUM>=5)
    {
        //第4套伺服
        sc_Dobot[4] =ecrt_master_slave_config(master, Dobot_Pos4, Dobot_Brand4);
        if (!sc_Dobot[4]) {
            //fprintf(stderr, "Failed to get slave configuration Servo J4.\n");
            std::cout<<"Failed to get slave configuration Servo J4"<<std::endl;
            return -1;
        }

        if (ecrt_slave_config_pdos(sc_Dobot[4], EC_END, Dobot_syncs)) {
            //fprintf(stderr, "Failed to configure PDOs. Servo J4.\n");
            std::cout<<"Failed to configure PDOs. Servo J4."<<std::endl;
            return -1;
        }
    }

    if(GLOBAL_AXIS_NUM>=6)
    {
        //第5套伺服
        sc_Dobot[5] =ecrt_master_slave_config(master, Dobot_Pos5, Dobot_Brand5);
        if (!sc_Dobot[5]) {
            //fprintf(stderr, "Failed to get slave configuration Servo J5.\n");
            std::cout<<"Failed to get slave configuration Servo J5"<<std::endl;
            return -1;
        }

        if (ecrt_slave_config_pdos(sc_Dobot[5], EC_END, Dobot_syncs)) {
            //fprintf(stderr, "Failed to configure PDOs. Servo J5.\n");
            std::cout<<"Failed to configure PDOs. Servo J5."<<std::endl;
            return -1;
        }

    }

    //只在特定情况下是引入这个数字模块的
    if(DIDO_LAST==1)
    {
        //        sc_Dobot_IO[2] =ecrt_master_slave_config(master, Dobot_IOPos2, Dobot_IO2);
        //        if (!sc_Dobot_IO[2]) {
        //            std::cout<<"Failed to get slave configuration IO 2"<<std::endl;
        //            return -1;
        //        }

        //        if (ecrt_slave_config_pdos(sc_Dobot_IO[2], EC_END, slave_8_syncs)) {
        //            std::cout<<"Failed to configure PDOs.  IO 2"<<std::endl;
        //            return -1;
        //        }

    }


    if (ecrt_domain_reg_pdo_entry_list(domainServoOutput, domainServoOutput_regs))
    {
        std::cout<<"PDO entry registration failed domainServoOutput_regs !"<<std::endl;
        return -1;
    }
    if (ecrt_domain_reg_pdo_entry_list(domainServoInput, domainServoInput_regs))
    {
        std::cout<<"PDO entry registration failed! domainServoInput_regs"<<std::endl;
        return -1;
    }

    return 0;
}


void rt_check_domain_state(void)
{
    ec_domain_state_t ds = {};
    ec_domain_state_t ds1 = {};

    //domainServoInput
    ecrt_domain_state(domainServoInput, &ds);
    if (ds.working_counter != domainServoInput_state.working_counter)
    {
        //rt_printf("domainServoInput: WC %u.\n", ds.working_counter);
        std::cout<<"domainServoInput: WC "<<ds.working_counter<<std::endl;
    }
    if (ds.wc_state != domainServoInput_state.wc_state)
    {
        //rt_printf("domainServoInput: State %u.\n", ds.wc_state);
        std::cout<<"domainServoInput: State "<<ds.wc_state<<std::endl;
    }
    domainServoInput_state = ds;

    //domainServoOutput
    ecrt_domain_state(domainServoOutput, &ds1);
    if (ds1.working_counter != domainServoOutput_state.working_counter)
    {
        //rt_printf("domainServoOutput: WC %u.\n", ds1.working_counter);
        std::cout<<"domainServoOutput: WC "<<ds.working_counter<<std::endl;

    }
    if (ds1.wc_state != domainServoOutput_state.wc_state)
    {
        //rt_printf("domainServoOutput: State %u.\n", ds1.wc_state);
        std::cout<<"domainServoOutput: State "<<ds.wc_state<<std::endl;
    }
    domainServoOutput_state = ds1;
}

/****************************************************************************/
void rt_check_master_state(void)
{
    ec_master_state_t ms;

    ecrt_master_state(master, &ms);

    if (ms.slaves_responding != master_state.slaves_responding)
    {
        //std::cout<<"rt_check_master_state ms.slaves_responding="<<ms.slaves_responding<<std::endl;
    }

    if (ms.al_states != master_state.al_states)
    {
        //std::cout<<"rt_check_master_state ms.al_states="<<ms.al_states<<std::endl;
    }

    if (ms.link_up != master_state.link_up)
    {
        //std::cout<<"rt_check_master_state ms.link_up="<<ms.link_up<<std::endl;
    }

    master_state = ms;
}

/****************************************************************************/
void check_slave_config_states(void)
{

    if(GLOBAL_AXIS_NUM>=1)
    {
        ec_slave_config_state_t s0;
        ecrt_slave_config_state(sc_Dobot[0],&s0);
        if (s0.al_state != sc_Dobot_state[0].al_state)
        {
            //std::cout<<"sc_Dobot_state[0]: State "<<s0.al_state<<std::endl;
        }

        if (s0.online != sc_Dobot_state[0].online)
        {
            //std::cout<<"sc_Dobot_state[0]: online "<<s0.online<<std::endl;
        }

        if (s0.operational != sc_Dobot_state[0].operational)
        {
            //std::cout<<"sc_Dobot_state[0]: operational "<<s0.operational<<std::endl;
        }
        sc_Dobot_state[0] = s0;

    }

    if(GLOBAL_AXIS_NUM>=2)
    {
        ec_slave_config_state_t s1;
        ecrt_slave_config_state(sc_Dobot[1],&s1);
        if (s1.al_state != sc_Dobot_state[1].al_state)
        {
            //std::cout<<"sc_Dobot_state[1]: State "<<s1.al_state<<std::endl;
        }

        if (s1.online != sc_Dobot_state[1].online)
        {
            //std::cout<<"sc_Dobot_state[1]: online "<<s1.online<<std::endl;
        }

        if (s1.operational != sc_Dobot_state[1].operational)
        {
            //std::cout<<"sc_Dobot_state[1]: operational "<<s1.operational<<std::endl;
        }
        sc_Dobot_state[1] = s1;

    }

    if(GLOBAL_AXIS_NUM>=3)
    {
        ec_slave_config_state_t s2;
        ecrt_slave_config_state(sc_Dobot[2],&s2);
        if (s2.al_state != sc_Dobot_state[2].al_state)
        {
            //std::cout<<"sc_Dobot_state[2]: State "<<s2.al_state<<std::endl;
        }

        if (s2.online != sc_Dobot_state[2].online)
        {
            //std::cout<<"sc_Dobot_state[2]: online "<<s2.online<<std::endl;
        }

        if (s2.operational != sc_Dobot_state[2].operational)
        {
            //std::cout<<"sc_Dobot_state[2]: operational "<<s2.operational<<std::endl;
        }
        sc_Dobot_state[2] = s2;
    }

    if(GLOBAL_AXIS_NUM>=4)
    {
        ec_slave_config_state_t s3;
        ecrt_slave_config_state(sc_Dobot[3],&s3);
        if (s3.al_state != sc_Dobot_state[3].al_state)
        {
            //std::cout<<"sc_Dobot_state[3]: State "<<s3.al_state<<std::endl;
        }

        if (s3.online != sc_Dobot_state[3].online)
        {
            //std::cout<<"sc_Dobot_state[3]: online "<<s3.online<<std::endl;
        }

        if (s3.operational != sc_Dobot_state[3].operational)
        {
            //std::cout<<"sc_Dobot_state[3]: operational "<<s3.operational<<std::endl;
        }
        sc_Dobot_state[3] = s3;
    }

    if(GLOBAL_AXIS_NUM>=5)
    {
        ec_slave_config_state_t s4;
        ecrt_slave_config_state(sc_Dobot[4],&s4);
        if (s4.al_state != sc_Dobot_state[4].al_state)
        {
            //std::cout<<"sc_Dobot_state[4]: State "<<s4.al_state<<std::endl;
        }

        if (s4.online != sc_Dobot_state[4].online)
        {
            //std::cout<<"sc_Dobot_state[4]: online "<<s4.online<<std::endl;
        }

        if (s4.operational != sc_Dobot_state[4].operational)
        {
            //std::cout<<"sc_Dobot_state[4]: operational "<<s4.operational<<std::endl;
        }
        sc_Dobot_state[4] = s4;
    }

    if(GLOBAL_AXIS_NUM>=6)
    {
        ec_slave_config_state_t s5;
        ecrt_slave_config_state(sc_Dobot[5],&s5);
        if (s5.al_state != sc_Dobot_state[5].al_state)
        {
            //std::cout<<"sc_Dobot_state[5]: State "<<s5.al_state<<std::endl;
        }

        if (s5.online != sc_Dobot_state[5].online)
        {
            //std::cout<<"sc_Dobot_state[5]: online "<<s5.online<<std::endl;
        }

        if (s5.operational != sc_Dobot_state[5].operational)
        {
            //std::cout<<"sc_Dobot_state[5]: operational "<<s5.operational<<std::endl;
        }
        sc_Dobot_state[5] = s5;

    }


    if(DIDO_FIRST==1)
    {
        ec_slave_config_state_t s0;
        ecrt_slave_config_state(sc_Dobot_IO[0],&s0);
        if (s0.al_state != sc_Dobot_IO_state[0].al_state)
        {
            //std::cout<<"sc_Dobot_state[0]: State "<<s0.al_state<<std::endl;
        }

        if (s0.online != sc_Dobot_IO_state[0].online)
        {
            //std::cout<<"sc_Dobot_state[0]: online "<<s0.online<<std::endl;
        }

        if (s0.operational != sc_Dobot_IO_state[0].operational)
        {
            //std::cout<<"sc_Dobot_state[0]: operational "<<s0.operational<<std::endl;
        }
        sc_Dobot_IO_state[0] = s0;
    }

    if(DIDO_SECODE==1)
    {
        ec_slave_config_state_t s1;
        ecrt_slave_config_state(sc_Dobot_IO[1],&s1);
        if (s1.al_state != sc_Dobot_IO_state[1].al_state)
        {
            //std::cout<<"sc_Dobot_state[1]: State "<<s1.al_state<<std::endl;
        }

        if (s1.online != sc_Dobot_IO_state[1].online)
        {
            //std::cout<<"sc_Dobot_state[1]: online "<<s1.online<<std::endl;
        }

        if (s1.operational != sc_Dobot_IO_state[1].operational)
        {
            //std::cout<<"sc_Dobot_state[1]: operational "<<s1.operational<<std::endl;
        }
        sc_Dobot_IO_state[1] = s1;
    }




}

/****************************************************************************/
void ReleaseMaster()
{
    if(master)
    {
        //printf("xenomai End of Program, release master\n");
        //std::cout<<"xenomai End of Program, release master"<<std::endl;
        ecrt_release_master(master);
        master = NULL;
    }
}

/****************************************************************************/
int ActivateMaster()
{
    int ret;
    //printf("xenomai Requesting master...\n");
    //std::cout<<"xenomai Requesting master GLOBAL_AXIS_NUM="<<GLOBAL_AXIS_NUM<<std::endl;

    if(master)
        return 0;
    master = ecrt_request_master(0);
    if (!master) {
        return -1;
    }

    //if (ecrt_slave_config_pdos(sc_Dobot[0], EC_END, Dobot_syncs)) {
    ConfigPDO();


    ecrt_slave_config_sdo16(sc_Dobot[0],0x1c32,01,2);
    ecrt_slave_config_sdo16(sc_Dobot[0],0x1c33,01,2);

    ecrt_slave_config_sdo16(sc_Dobot[1],0x1c32,01,2);
    ecrt_slave_config_sdo16(sc_Dobot[1],0x1c33,01,2);

    ecrt_slave_config_sdo16(sc_Dobot[2],0x1c32,01,2);
    ecrt_slave_config_sdo16(sc_Dobot[2],0x1c33,01,2);

    ecrt_slave_config_sdo16(sc_Dobot[3],0x1c32,01,2);
    ecrt_slave_config_sdo16(sc_Dobot[3],0x1c33,01,2);

    //ecrt_slave_config_sdo16 will resulet to error
    //ecrt_slave_config_sdo8(sc_Dobot[0],0x2390,02,1);
    //ecrt_slave_config_sdo8(sc_Dobot[1],0x2390,02,1);
    //ecrt_slave_config_sdo8(sc_Dobot[2],0x2390,02,1);
    //ecrt_slave_config_sdo8(sc_Dobot[3],0x2390,02,1);
    //ecrt_slave_config_sdo32(sc_Dobot[0],index,subindex,dataToWrite);

    //ret = ecrt_master_select_reference_clock(master,sc_Dobot_IO[0]);
    //ret = ecrt_master_select_reference_clock(master,sc_Dobot[0]);

    //    if(DIDO_FIRST==1)
    //    {
    //        ret = ecrt_master_select_reference_clock(master,sc_Dobot_IO[0]);
    //    }
    //    else if(DIDO_SECODE==1)
    //    {
    //        ret = ecrt_master_select_reference_clock(master,sc_Dobot_IO[1]);
    //    }
    //    else
    //    {
    //        ret =ecrt_master_select_reference_clock(master,sc_Dobot[0]);
    //    }

    ret =ecrt_master_select_reference_clock(master,sc_Dobot[0]);
    if (ret < 0)
    {
        printf("Failed to select reference clock\n");
    }


    if(DIDO_FIRST==1)
    {
        ecrt_slave_config_dc(sc_Dobot_IO[0], 0x0300, PERIOD_NS, PERIOD_NS_HALF, 0, 0);//500000
    }

    if(DIDO_SECODE==1)
    {
        ecrt_slave_config_dc(sc_Dobot_IO[1], 0x0300, PERIOD_NS, PERIOD_NS_HALF, 0, 0);//500000
    }

    //robot io board
    if(DIDO_LAST==1)
    {
        ecrt_slave_config_dc(sc_Dobot_IO[2], 0x0300, PERIOD_NS, PERIOD_NS_HALF, 0, 0);//500000
    }

    if(GLOBAL_AXIS_NUM>=1)
    {
        ecrt_slave_config_dc(sc_Dobot[0], 0x0300, PERIOD_NS, PERIOD_NS_HALF, 0, 0);
    }
    if(GLOBAL_AXIS_NUM>=2)
    {
        ecrt_slave_config_dc(sc_Dobot[1], 0x0300, PERIOD_NS, PERIOD_NS_HALF, 0, 0);
    }
    if(GLOBAL_AXIS_NUM>=3)
    {
        ecrt_slave_config_dc(sc_Dobot[2], 0x0300, PERIOD_NS, PERIOD_NS_HALF, 0, 0);
    }
    if(GLOBAL_AXIS_NUM>=4)
    {
        ecrt_slave_config_dc(sc_Dobot[3], 0x0300, PERIOD_NS, PERIOD_NS_HALF, 0, 0);
    }
    if(GLOBAL_AXIS_NUM>=5)
    {
        ecrt_slave_config_dc(sc_Dobot[4], 0x0300, PERIOD_NS, PERIOD_NS_HALF, 0, 0);
    }
    if(GLOBAL_AXIS_NUM>=6)
    {
        ecrt_slave_config_dc(sc_Dobot[5], 0x0300, PERIOD_NS, PERIOD_NS_HALF, 0, 0);
    }

    //std::cout<<"xenomai Activating master"<<std::endl;
    if (ecrt_master_activate(master)) {
        //printf("xenomai Activating master...failed\n");
        std::cout<<"xenomai Activating master...failed"<<std::endl;
        return -1;
    }
    if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
        //fprintf(stderr, "xenomai Failed to get domain data pointer.\n");
        std::cout<<"xenomai Failed to get domain data pointer"<<std::endl;

        return -1;
    }
    if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
        //fprintf(stderr, "xenomai Failed to get domain data pointer.\n");
        std::cout<<"xenomai Failed to get domain data pointer"<<std::endl;

        return -1;
    }

    //std::cout<<"xenomai Activating master...success PERIOD_NS_HALF="<<PERIOD_NS_HALF<<std::endl;

    return 0;
}


//更新软件算法
void RobotFunction()
{
    functiont::instance()->MAINLOOP();

}


struct timespec timespec_add(struct timespec time1, struct timespec time2)
{
    struct timespec result;

    if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) {
        result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
    } else {
        result.tv_sec = time1.tv_sec + time2.tv_sec;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
    }

    return result;
}

ThreadObject::ThreadObject(QObject *parent):QObject(parent)
  ,m_runCount(10)
  ,m_runCount2(std::numeric_limits<int>::max())
  ,m_isStop(true)
{
    //std::cout<<"ThreadObject::runETHERCATMASTER GLOBAL_AXIS_NUM="<<GLOBAL_AXIS_NUM<<std::endl;
    //std::cout<<"ThreadObject::runETHERCATMASTER PERIOD_NS="<<PERIOD_NS<<std::endl;

    if(true)
    {
        gSysRunning.m_gWorkStatus = SYS_WORKING_POWER_ON;
        if(gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)
        {
            //实例化的时候配置和激活主站
            ActivateMaster();
            ecstate = 0;
            gSysRunning.m_gWorkStatus = SYS_WORKING_SAFE_MODE;
            //std::cout<<"xenomai SYS_WORKING_SAFE_MODE"<<std::endl;
        }
    }

}

ThreadObject::~ThreadObject()
{
    qDebug() << "ThreadObject destroy";
    emit message(QString("Destroy %1->%2,thread id:%3").arg(__FUNCTION__).arg(__FILE__).arg((uint64_t)QThread::currentThreadId()));
}

void ThreadObject::setRunCount(int count)
{
    m_runCount = count;
    emit message(QString("%1->%2,thread id:%3").arg(__FUNCTION__).arg(__FILE__).arg((uint64_t)QThread::currentThreadId()));
}



void ThreadObject::runSomeBigWork1()
{

}

//cyclic_task();
static uint64_t run_status = 0;
void ThreadObject::runSomeBigWork2()
{
    //std::cout<<"ThreadObject::runSomeBigWork2(ETHERCAT) START IfWorkModeReal="<<fun_globalvariable::instance()->IfWorkModeReal<<std::endl;

    if(true)
    {
        pid_t pid = getpid();
        if (setpriority(PRIO_PROCESS, pid, -19))
            fprintf(stderr, "Warning: Failed to set priority: %s\n",strerror(errno));

        /* Create cyclic RT-thread */
        struct sched_param param;
        param.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1;
        if (sched_setscheduler(0, SCHED_FIFO, &param) == -1) {
            printf("ERROR IN SETTING THE SCHEDULER");
            perror("errno");
            //return -1;
        }

        if(true)
        {
            if(DO_MEASURE==1)
            {
                logInit();
            }

            cpu_set_t cpuset;
            CPU_ZERO(&cpuset);
            CPU_SET(3, &cpuset);
            if (sched_setaffinity(getpid(), sizeof(cpuset), &cpuset) == -1) {
                printf("Set CPU affinity failue, ERROR:%s\n", strerror(errno));
                //return -1;
            }
        }

    }

    struct timespec sleep_time   =   {0, 1000000};
    /*********************DC ref slave *******************************/

    //MAC地址绑定的序列号机制(要转换成小写)
    NetWorkInfo iptarget = functiont::instance()->GetTargetIPInfo(1);
    std::cout<<"your MAC="<<iptarget.MAC.toStdString()<<std::endl;
    if(true)
    {
        set_dc_time();
        first_start_time(master, PERIOD_NS);
    }
    else
    {
        first_start_time(master, PERIOD_NS);
    }

    //while (true)//run
    while(1)
    {
        //从实际硬件更新输入
        if(true)//GLOBAL_WORKMODE_REAL==true
        {
            //DriverEtherCAT();
            static int cycle_counter = 0;
            cycle_counter++;

            // receive process data
            ecatPerfMeasStart (4);

            // receive EtherCAT frames
            ecrt_master_receive(master);

            ecatPerfMeasEnd (4);

            ecrt_domain_process(domainServoOutput);
            ecrt_domain_process(domainServoInput);
            //rt_check_domain_state();
            ecatPerfMeasStart(0);

            //            // write process data
            //            if(run_status < 9001) {
            //                //set_ctrl_wd(run_status);
            //                run_status++;
            //            }

            //状态机操作
            switch (gSysRunning.m_gWorkStatus)
            {
            //这是SAFE OP模式
            case SYS_WORKING_SAFE_MODE:{
                //检查主站是否处于 OP 模式, 若不是，则调整为 OP 模式
                rt_check_master_state();
                check_slave_config_states();

                //std::cout<<"master_state.al_states="<<master_state.al_states<<std::endl;
                //std::cout<<"sc_Dobot_state[0].al_state="<<sc_Dobot_state[0].al_state<<std::endl;


                if((master_state.al_states & ETHERCAT_STATUS_OP))
                {
                    bool tmp = true;

                    //如果有伺服，则每个从站伺服检测数量
                    if(GLOBAL_AXIS_NUM>0)
                    {
                        //这里for循环检测每个从站状态了GLOBAL_AXIS_NUM
                        for(int i=0;i<GLOBAL_AXIS_NUM;i++)//GLOBAL_AXIS_NUM
                        {
                            if(sc_Dobot_state[i].al_state != ETHERCAT_STATUS_OP)
                            {
                                tmp = false;
                                break ;
                            }
                        }

                        if(tmp==true)
                        {
                            //change state to op
                            ecstate = 0;
                            gSysRunning.m_gWorkStatus = SYS_WORKING_OP_MODE;
                            std::cout<<"xenomai SYS_WORKING_OP_MODE"<<std::endl;
                        }
                    }
                    //如果没有伺服，则检查每块从站IO板
                    else
                    {
                        if(DIDO_FIRST==1)
                        {
                            if(sc_Dobot_IO_state[0].al_state != ETHERCAT_STATUS_OP)
                            {
                                tmp = false;
                                break ;
                            }
                        }

                        if(DIDO_SECODE==1)
                        {
                            if(sc_Dobot_IO_state[1].al_state != ETHERCAT_STATUS_OP)
                            {
                                tmp = false;
                                break ;
                            }

                        }

                        if(tmp==true)
                        {
                            //change state to op
                            ecstate = 0;
                            gSysRunning.m_gWorkStatus = SYS_WORKING_OP_MODE;
                            std::cout<<"xenomai SYS_WORKING_OP_MODE"<<std::endl;
                        }


                    }

                }
            }break;

                //这是在切伺服上使能的模式
            case SYS_WORKING_OP_MODE:
            {
                if(GLOBAL_AXIS_NUM>0)
                {
                    //始终需要获取每个关节的状态
                    if(true)
                    {
                        for(int i=0;i<GLOBAL_AXIS_NUM;i++)
                        {
                            functiont::instance()->RobotHardwareInfo.Statusword.DATA[i]=EC_READ_U16(domainInput_pd + statusword[i]);
                            //fun_globalvariable::instance()->RobotHardwareInfo.Errorcode.DATA[i]=EC_READ_U16(domainInput_pd + errorcode[i]);

                            functiont::instance()->RobotHardwareInfo.CurrentPosition.DATA[i]=EC_READ_S32(domainInput_pd + actpos[i]);
                            //fun_globalvariable::instance()->RobotHardwareInfo.CurrentVelocity.DATA[i]=EC_READ_S32(domainInput_pd + actvel[i]);
                            functiont::instance()->RobotHardwareInfo.CurrentTorque.DATA[i]=EC_READ_S16(domainInput_pd + acttorque[i]);
                            //fun_globalvariable::instance()->RobotHardwareInfo.CurrentPositionFollowError.DATA[i]=EC_READ_S32(domainInput_pd + actposfollowerror[i]);
                        }

                    }

                    static int startpos[10];
                    //main loop
                    // if(fun_globalvariable::instance()->CurrentJob==1)
                    // {
                    //     for(int i=0;i<GLOBAL_AXIS_NUM;i++)
                    //     {
                    //         EC_WRITE_U16(domainOutput_pd + controlword[i], 0x80);
                    //     }
                    //     fun_globalvariable::instance()->CurrentJobString="ClearError";
                    // }

                    //do power on
                    if(functiont::instance()->job==1)
                    {
                        ecstate++;
                        //std::cout<<"ecstate = "<<ecstate<<std::endl;
                        //使能伺服
                        if(ecstate <= 1000)
                        {

                            switch (ecstate)
                            {
                            case 1:

                                //all
                                if(functiont::instance()->DoPowerONIndex==-1)
                                {
                                    for(int i=0;i<GLOBAL_AXIS_NUM;i++)
                                    {
                                        //EC_WRITE_U8(domainOutput_pd + modeofoperation[i], 8);
                                    }
                                }
                                //single axis
                                else
                                {
                                    //EC_WRITE_U8(domainOutput_pd + modeofoperation[fun_globalvariable::instance()->DoPowerONIndex], 8);
                                }

                                break;
                            case 200:
                                //all
                                if(functiont::instance()->DoPowerONIndex==-1)
                                {
                                    for(int i=0;i<GLOBAL_AXIS_NUM;i++)
                                    {
                                        EC_WRITE_U16(domainOutput_pd + controlword[i], 0x80);
                                    }
                                }
                                //single axis
                                else
                                {
                                    EC_WRITE_U16(domainOutput_pd + controlword[functiont::instance()->DoPowerONIndex], 0x80);
                                }

                                break;
                            case 300:
                                //all
                                if(functiont::instance()->DoPowerONIndex==-1)
                                {
                                    for(int i=0;i<GLOBAL_AXIS_NUM;i++)
                                    {
                                        startpos[i] = EC_READ_S32(domainInput_pd + actpos[i]);//获取当前位置并且写入
                                        EC_WRITE_S32(domainOutput_pd + targetposition[i], EC_READ_S32(domainInput_pd + actpos[i]));
                                    }
                                }
                                //single axis
                                else
                                {
                                    startpos[functiont::instance()->DoPowerONIndex] = EC_READ_S32(domainInput_pd + actpos[functiont::instance()->DoPowerONIndex]);//获取当前位置并且写入
                                    EC_WRITE_S32(domainOutput_pd + targetposition[functiont::instance()->DoPowerONIndex], EC_READ_S32(domainInput_pd + actpos[functiont::instance()->DoPowerONIndex]));
                                }

                                break;
                            case 400:
                                //all
                                if(functiont::instance()->DoPowerONIndex==-1)
                                {
                                    for(int i=0;i<GLOBAL_AXIS_NUM;i++)
                                    {
                                        EC_WRITE_U16(domainOutput_pd + controlword[i], 0x06);
                                    }
                                }
                                //single axis
                                else
                                {
                                    EC_WRITE_U16(domainOutput_pd + controlword[functiont::instance()->DoPowerONIndex], 0x06);
                                }

                                break;
                            case 500:
                                //all
                                if(functiont::instance()->DoPowerONIndex==-1)
                                {
                                    for(int i=0;i<GLOBAL_AXIS_NUM;i++)
                                    {
                                        EC_WRITE_U16(domainOutput_pd + controlword[i], 0x07);
                                    }
                                }
                                //single axis
                                else
                                {
                                    EC_WRITE_U16(domainOutput_pd + controlword[functiont::instance()->DoPowerONIndex], 0x07);
                                }

                                break;
                            case 600:
                                //all
                                if(functiont::instance()->DoPowerONIndex==-1)
                                {
                                    for(int i=0;i<GLOBAL_AXIS_NUM;i++)
                                    {
                                        EC_WRITE_U16(domainOutput_pd + controlword[i], 0xF);
                                    }
                                }
                                //single axis
                                else
                                {
                                    EC_WRITE_U16(domainOutput_pd + controlword[functiont::instance()->DoPowerONIndex], 0xF);
                                }

                                break;
                            }

                        }
                        else
                        {
                            functiont::instance()->job=0;
                            ecstate=0;
                        }

                    }

                    // //do power off
                    else if(functiont::instance()->job==-1)
                    {
                        //all
                        if(functiont::instance()->DoPowerONIndex==-1)
                        {
                            for(int i=0;i<GLOBAL_AXIS_NUM;i++)
                            {
                                EC_WRITE_U16(domainOutput_pd + controlword[i], 0x06);
                            }
                        }
                        //single axis
                        else
                        {
                            EC_WRITE_U16(domainOutput_pd + controlword[functiont::instance()->DoPowerONIndex], 0x06);
                        }
                        ecstate=0;

                    }

                    if(true)
                    {
                        if(true)
                        {
                            for(int i=0;i<GLOBAL_AXIS_NUM;i++)
                            {
                                EC_WRITE_S32(domainOutput_pd + targetposition[i], (functiont::instance()->RobotHardwareInfo.TargetPosition.DATA[i]));
                            }
                        }
                    }

                }



            }break;

                //这里就开始写入目标位置，让电机转动了
            default:
            {
                std::cout<<"should not in this state"<<std::endl;

            }break;

            }

            ecatPerfMeasEnd(0);

            //每隔一定时间检测主站和从站状态
            if (!(cycle_counter % 50))
            {
                rt_check_master_state();
                check_slave_config_states();

                //std::cout<<"CurrentPosition="<<functiont::instance()->RobotHardwareInfo.CurrentPosition.DATA[0]<<std::endl;
                //std::cout<<"ETHERCAT CurrentPosition="<<fun_globalvariable::instance()->RobotHardwareInfo.CurrentPosition.DATA[0]<<","<<fun_globalvariable::instance()->RobotHardwareInfo.CurrentPosition.DATA[1]<<","<<fun_globalvariable::instance()->RobotHardwareInfo.CurrentPosition.DATA[2]<<","<<fun_globalvariable::instance()->RobotHardwareInfo.CurrentPosition.DATA[3]<<std::endl;
                //std::cout<<"ETHERCAT CurrentTorque="<<fun_globalvariable::instance()->RobotHardwareInfo.CurrentTorque.DATA[0]<<","<<fun_globalvariable::instance()->RobotHardwareInfo.CurrentTorque.DATA[1]<<","<<fun_globalvariable::instance()->RobotHardwareInfo.CurrentTorque.DATA[2]<<","<<fun_globalvariable::instance()->RobotHardwareInfo.CurrentTorque.DATA[3]<<std::endl;

                //                if(fabs(fun_globalvariable::instance()->RobotHardwareInfo.CurrentTorque.DATA[0])>300)
                //                {
                //                    std::cout<<"ETHERCAT CurrentTorque ON!!!"<<std::endl;

                //                }
                //                else
                //                {
                //                    std::cout<<"ETHERCAT CurrentTorque OFF!!!"<<std::endl;

                //                }



            }

            if (!(cycle_counter % 500))
            {

                //ETHERCAT CurrentPosition=-50330128,-2528363,-1769866,-4125381
                std::cout<<"ETHERCAT CurrentPosition="<<functiont::instance()->RobotHardwareInfo.CurrentPosition.DATA[0]<<","<<functiont::instance()->RobotHardwareInfo.CurrentPosition.DATA[1]<<","<<functiont::instance()->RobotHardwareInfo.CurrentPosition.DATA[2]<<","<<functiont::instance()->RobotHardwareInfo.CurrentPosition.DATA[3]<<std::endl;
                //std::cout<<"ETHERCAT CurrentTorque="<<fun_globalvariable::instance()->RobotHardwareInfo.CurrentTorque.DATA[0]<<","<<fun_globalvariable::instance()->RobotHardwareInfo.CurrentTorque.DATA[1]<<","<<fun_globalvariable::instance()->RobotHardwareInfo.CurrentTorque.DATA[2]<<","<<fun_globalvariable::instance()->RobotHardwareInfo.CurrentTorque.DATA[3]<<std::endl;

                //                if(fabs(fun_globalvariable::instance()->RobotHardwareInfo.CurrentTorque.DATA[0])>300)
                //                {
                //                    std::cout<<"ETHERCAT CurrentTorque ON!!!"<<std::endl;

                //                }
                //                else
                //                {
                //                    std::cout<<"ETHERCAT CurrentTorque OFF!!!"<<std::endl;

                //                }



            }
            // send process data
            ecrt_domain_queue(domainServoOutput);
            ecrt_domain_queue(domainServoInput);

            wait_send(master, PERIOD_NS);

            ecatPerfRMS(5);
            ecatPerfMeasStart (3);
            sync_dc_time(master);
            ecrt_master_send(master);
            ecatPerfMeasEnd (3);
            calc_system_diff_time();
            ecatPerfMeasStart (2);
            wait_recv(&sleep_time);

            ecatPerfMeasEnd (2);

        }
        else
        {
            //虚拟模式下也要做延时
            struct timespec wakeupTime, time;
            const struct timespec cycletime = {0, PERIOD_NS};
            clock_gettime(CLOCK_TO_USE, &wakeupTime);
            wakeupTime = timespec_add(wakeupTime, cycletime);
            clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
        }

        //算法需要始终更新
        RobotFunction();

    }
}


void ThreadObject::stop()
{
    QMutexLocker locker(&m_stopMutex);
    emit message(QString("%1->%2,thread id:%3").arg(__FUNCTION__).arg(__FILE__).arg((uint64_t)QThread::currentThreadId()));
    m_isStop = true;
}
